//------------------------------------------------------------------------------
//  <copyright file="FRIdispatcher.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

#pragma unmanaged
#include "Stdafx.h"
#include <memory.h>
#include <KukaLbr4Definitions.h>
#include "FRIdispatcher.h"

using namespace Microsoft::Robotics::Hardware::KukaLBR;

/// <summary> Initializes a new instance of the <c>FRIdispatcher</c> for dispatching callbacks to a managed wrapper. </summary>
/// <param name="pfnInitializeFri"> Callback to initialize the FRI</param>
/// <param name="pfnCloseFri">Callback to  to close the FRI  </param>
/// <param name="pfnWaitFri">Callback to wait on FRI connection </param>
/// <param name="pfnDoNothingFri">Callback to do nothing  </param>
/// <param name="pfnGetRequestFri">Callback to get FRI request  </param>
/// <param name="pfnJointControlFr">Callback to do joint control  </param>
/// <param name="pfnJointImpedanceControlFri">Callback for joint impedance control </param>
/// <param name="pfnCartesianImpedanceControlFri">Callback for cartesian control  </param>
/// <param name="pfnKrlCartesianImpedanceControlFri">Callback for cartesian control using Krl motion interpreter</param>
FRIdispatcher::FRIdispatcher(
    FRICALLBACK0 pfnInitializeFri,
    FRICALLBACK0 pfnCloseFri,
    FRICALLBACK0 pfnWaitFri,
    FRICALLBACK0 pfnDoNothingFri,
    FRICALLBACKPPD1 pfnGetRequestFri,
    FRICALLBACKPD1 pfnJointControlFri,
    FRICALLBACKPD4 pfnJointImpedanceControlFri,
    FRICALLBACKPD4 pfnCartesianImpedanceControlFri,
	FRICALLBACKPD1I1 pfnDoKrlCartesianImpedanceControlFri)
{
    this->pRegisters = new ControllerStateMachineRegisters<double>(
		KukaLBR4PlusFeedbackRegisters::NumberOfFeedbackRegisters,
		(unsigned int *)&(KukaLBR4PlusRegisterSizes[KukaLBR4PlusWorkingRegisters::NumberOfWorkingRegisters]));

    // Set up default register values
    *(this->pRegisters->GetRegister(KukaLBR4PlusFeedbackRegisters::CycleTimeInMsRegister)) = 25;

    this->pfnInitializeFri = pfnInitializeFri;
    this->pfnCloseFri = pfnCloseFri;
    this->pfnWaitFri = pfnWaitFri;
    this->pfnDoNothingFri = pfnDoNothingFri;
    this->pfnGetRequestFri = pfnGetRequestFri;
    this->pfnJointControlFri = pfnJointControlFri;
    this->pfnJointImpedanceControlFri = pfnJointImpedanceControlFri;
    this->pfnCartesianImpedanceControlFri = pfnCartesianImpedanceControlFri;
	this->pfnKrlCartesianImpedanceControlFri = pfnDoKrlCartesianImpedanceControlFri;
}

FRIdispatcher::~FRIdispatcher()
{
    delete this->pRegisters;
}

/// <summary> Send a response to the FRI request, but take no control actions in it. </summary>
/// <returns> Status code.  0 on no error. </returns>
int FRIdispatcher::DoNothing()
{
    return this->pfnDoNothingFri();
}

/// <summary> Execute joint position control. </summary>
/// <param name="jointPosition"> Array of joint positions. </param>
/// <returns> Status code.  0 on no error. </returns>
int FRIdispatcher::DoPositionControl(double *jointPosition)
{
	return this->pfnJointControlFri(jointPosition);
}

/// <summary> Execute Cartesian impedance control. </summary>
/// <param name="cartesianPosition"> Array indicating desired cartesian position relative to base frame. </param>
/// <param name="cartesianStiffness"> Array of Cartesian stiffness values. </param>
/// <param name="cartesianDamping"> Array of Cartesian damping values. </param>
/// <param name="additionalTCPForceTorque"> Array of additional TCP force torque values. </param>
int FRIdispatcher::DoCartesianImpedanceControl(
	double *cartesianPosition,
	double *cartesianStiffness,
	double *cartesianDamping,
	double *additionalTCPForceTorque)
{
    int response = pfnCartesianImpedanceControlFri(cartesianPosition, cartesianStiffness, cartesianDamping, additionalTCPForceTorque);
    return response;
}

/// <summary> Wait on a new FRI request. </summary>
/// <returns> State of the LBR as indicated by the next FRI request. </returns>
int FRIdispatcher::GetFRIRequest(double **ppFeedbackRegisters)
{
    int response = this->pfnGetRequestFri(ppFeedbackRegisters);
    return response;
}

/// <summary> Initialize the FRI server. </summary>
/// <returns> Status code.  0 on no error. </returns>
int FRIdispatcher::InitializeFRI()
{
    return this->pfnInitializeFri();
}

/// <summary> Shut down the FRI server. </summary>
/// <returns> Status code.  0 on no error. </returns>
int FRIdispatcher::CloseFRI()
{
	return this->pfnCloseFri();
}

/// <summary> Wait for an FRI client to connect. </summary>
/// <returns> Status code.  0 on no error. </returns>
int FRIdispatcher::WaitOnFRIConnect()
{
	return this->pfnWaitFri();
}

/// <summary> Execute joint impedance control. </summary>
/// <param name="jointPosition"> Array of joint positions. </param>
/// <param name="jointStiffness"> Array of joint stiffness values. </param>
/// <param name="jointDamping"> Array of joint damping values. </param>
/// <param name="jointAdditionalTorque"> Array of addition joint torque values. </param>
/// <returns> Status code.  0 on no error. </returns>
int FRIdispatcher::DoJointImpedanceControl(
	double *jointPosition,
	double *jointStiffness,
	double *jointDamping,
	double *jointAdditionalTorque)
{
	return this->pfnJointImpedanceControlFri(jointPosition, jointStiffness, jointDamping, jointAdditionalTorque);
}

/// <summary> Gets the quality of the FRI connection </summary>
/// <returns> Perfect quality. </returns>
FRIQuality FRIdispatcher::GetFRIQuality()
{
	return FRIQuality::FRIQualityPerfect;
}

/// <summary> Execute Cartesian impedance control by passing position goals to KRL script. </summary>
/// <param name="pCartesianPosition"> Array indicating desired cartesian position of TCP (tool control point) relative to base frame. </param>
/// <param name="sequenceNumber"> Used to identify when a new position command is ready to be executed. </param>
int FRIdispatcher::DoKrlCartesianImpedanceControl(double *pCartesianPosition, int sequenceNumber)
{
	return this->pfnKrlCartesianImpedanceControlFri(pCartesianPosition, sequenceNumber);
}
